clc;
clear;
close all;

set(groot, 'defaultLegendBox', 'off');
set(groot, 'defaultLineLineWidth', 1.2);
addpath("D:\Matlab_Workspace\psins250418");

load("avpref.mat");
load("imugps.mat");
load("output.txt");
load('pcAvpOut.mat');

myfig;
subplot(321);
plot(avpref(:,end), avpref(:,1:2) * 180 / pi); xygo('pr');
hold on; plot(output(:,end), output(:,1:2) * 180 / pi);
hold on; plot(avp(:,end), avp(:,1:2) * 180 / pi);
legend("ref-pit","ref-rol","stm-pit","stm-rol","pc-pit","pc-rol");

subplot(322);
plot(avpref(:,end), avpref(:,3) * 180 / pi); xygo('y');
hold on; plot(output(:,end), output(:,3) * 180 / pi);
hold on; plot(avp(:,end), avp(:,3) * 180 / pi);
legend("ref-yaw","stm-yaw","pc-yaw");

subplot(323);
plot(gps(:,end), gps(:,1:3)); xygo('vn');
hold on; plot(output(:,end), output(:,4:6));
legend("gps-vN","gps-vE","stm-vN","stm-vE");

subplot(324);
plot(gps(:,end), gps(:,6)); xygo('hgt');
hold on; plot(output(:,end), output(:,9));
legend("gps-pU","stm-pU");

subplot(325);
plot(output(:,end), output(:,10:12) * 180 / pi * 3600); xygo('eb');
% legend("gps-pU","stm-pU");

subplot(326);
plot(output(:,end), output(:,13:15) / 9.8 * 1e3); xygo('db');

figure;
plot(avpref(:,end), avpref(:,7)); hold on;
plot(output(:,end), output(:,7));

figure;
plot(avpref(:,end), avpref(:,8)); hold on;
plot(output(:,end), output(:,8));